#include "Motor.h"
void MotorStop(void);
void MotorCalCurSpeedProcess(void);
MotorState_t eMotorState = MotorStStop;
MotorRunState_t xMotorMode = {0};

void MotorUpdateCCR(void);

void MotorInit(void)
{
    xMotorMode.dir = 0;
    xMotorMode.controlMode = 1;
}

void MotorProcess(void)
{
    MotorCalCurSpeedProcess();//计算当前速度
}


MotorState_t eMotorStateMachine(void)
{
    switch (eMotorState)
    {
        case MotorStStop:
            PWM_OFF();
            // eMotorState = MotorStChargeReady;
            break;
        case MotorStChargeReady:
            //下管以50占空比输出PWM波
            PWM_CH1N_Ctrl(PWM_ON,1000);
            PWM_CH2N_Ctrl(PWM_ON,1000);
            PWM_CH3N_Ctrl(PWM_ON,1000);
            //设置自举电容充电计时器100ms
            setMotorDelayCnt(100);
            eMotorState = MotorStCharging;
            break;
        case MotorStCharging:
            //等待充电时间到
            if( !u32GetMotorDelayCnt() )
            {
                PWM_OFF();
                eMotorState = MotorStInit;
            }
            break;
        case MotorStInit:
            xBldcRunMode.u16HallVal = BldcGetHallValue();
            xBldcRunMode.ePhase = BldcHallValToPhase(xBldcRunMode.u16HallVal,xMotorMode.dir);
            if( PHASE_ERROR != xBldcRunMode.ePhase )
            {
                xMotorMode.dutyCycle = MIN_DUTY_CYCLE;
                xMotorMode.CCR = PWMSetDutyCycle(MIN_DUTY_CYCLE);//初始占空比为20
                BldcPwmControl(xBldcRunMode.ePhase);
                setMotorStartCnt(0);//重置自适应启动计时器，在1ms时基里自增
                xMotorMode.selfStartSwitchCnt = 0;//重置自适应启动换向计数器，在霍尔换向中断里自增
                eMotorState = MotorSelfStart;//开始自适应负载启动         
            }
            break;
        case MotorSelfStart://自适应负载启动,换向3次，每次换向间隔不得小于300ms
            if( xMotorMode.selfStartSwitchCnt >= 3 ){
                eMotorState = MotorStRun;
            }else{
                //自适应启动计时器在霍尔换向中断里会清零，300ms没清零
                if( u32GetMotorStartCnt() >= 300 ){
                    xMotorMode.dutyCycle += 5;//占空比每次加5
                    if( xMotorMode.dutyCycle <= SELF_START_MAX_DUTY_CYCLE ){
                        xMotorMode.CCR = PWMSetDutyCycle(xMotorMode.dutyCycle);
                    }else{
                        //加到最大限度，则认为电机启动失败
                        eMotorState = MotorStartFail;
                        protectSetFaultBit(Fault_MotorStartFail);
                    }
                }
            }
        case MotorStRun:
            xBldcRunMode.u16HallVal = BldcGetHallValue();
            xBldcRunMode.ePhase = BldcHallValToPhase(xBldcRunMode.u16HallVal,xMotorMode.dir);            
            BldcPwmControl(xBldcRunMode.ePhase);
            break;
        case MotorStartFail:
            PWM_OFF();
            break;
        default:
            break;
    }
	return eMotorState;
}
//计算当前速度
//speed(RPM) = 60*f/POLE_PAIR = 60/(T(s)*POLE_PAIR) = 60/(ePeriod*0.000002*2)
void MotorCalCurSpeedProcess(void)
{
    if( xBldcRunMode.OneEPeriodUpdated == 1 )//完成一个电周期换向
    {
        uint8_t i;
        uint32_t ePeriod = 0;//一个电周期计数值
        xBldcRunMode.OneEPeriodUpdated = 0;

        for( i=0;i<6;i++ )
        {
            ePeriod += xBldcRunMode.phaseTimeBuffer[i];//获取一个电周期的计数值，单位2us
        }
        /*speed(RPM) = 60*f/POLE_PAIR = 60/(T(s)*POLE_PAIR) = 60/(ePeriod*0.000002*POLE_PAIR)*/
        ePeriod = ePeriod * POLE_PAIR;
        xMotorMode.speedInstant = (uint16_t ) (30000000 / ePeriod);//RPM顺时值
        //xMotorMode.speedAct  = lowPassFilter(LowPassFilterK,xMotorMode.speedInstant,xMotorMode.speedAct);//速度滤波
        xMotorMode.speedAct = xMotorMode.speedInstant;
    }

    if( !u8GetMotorSwitchCnt() ){
        // if(xMotorMode.speedInstant){
        //     xMotorMode.speedInstant--;
        // }
        // if(xMotorMode.speedAct){
        //     xMotorMode.speedAct--;
        // }
        xMotorMode.speedAct = 0;
    }

}

void MotorSetCCR(uint16_t CCR)
{
    xMotorMode.CCR = CCR;
}

uint16_t MotorGetCCR(void)
{
    return xMotorMode.CCR;
}

void MotorSetState(MotorState_t eState)
{
    eMotorState = eState;
}

MotorState_t eMotorGetState(void)
{
    return eMotorState;
}

uint16_t u16MotorGetDutyCycle(void)
{
    return xMotorMode.openLoopDuty;
}

uint16_t u16MotorGetSpeed(void)
{
    return xMotorMode.closeLoopSpeed;
}

uint16_t u16GetControlMode(void)
{
    return xMotorMode.controlMode;
}
